/*
 * RWGPS.hpp
 *
 *  Created on: 04/04/2012
 *      Author: grcanosa
 */


#include "RWGPS.h"

using namespace std;
using namespace webots;



RWGPS::RWGPS()
{

}
RWGPS::~RWGPS()
{

}

void RWGPS::init(webots::Robot* R, ros::NodeHandle* n_in,string name)
{
  n = n_in;
  time_step = R->getBasicTimeStep();
  GPSdev = R->getGPS(name);
  GPSdev->enable(time_step);




  //ROS PUBLISHER CREATION
  std::stringstream PubName;
  //Publisher names
  string RobotName = R->getName();
  PubName << RobotName <<"/gps";
  pub = n->advertise<sensor_msgs::NavSatFix>(PubName.str(),1000);


  //ROS MESSAGES

  ROSGPS.header.seq = 0;
  ROSGPS.header.frame_id = "gps";
  ROSGPS.status.service = 1;
  ROSGPS.status.status = 1;







}



void RWGPS::iterate()
{
  ros::Time now = ros::Time::now();
  //DEPCAM IMAGE
  const double* gps_values = GPSdev->getValues();
  ROSGPS.header.stamp = now;
  ROSGPS.altitude = gps_values[2];
  ROSGPS.longitude = gps_values[0];
  ROSGPS.latitude = gps_values[1];
  ROSGPS.position_covariance_type = 1;
  //Write the position covariance

  pub.publish(ROSGPS);


}


